/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-11 17:25:50
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-06-04 10:43:04
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\pid\pid.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "pid.h"
#include "maths.h"
#include "fc_sensors.h"
#define DBG_TAG "pid"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
ZINO_CONFIG_REGISTER(pid_cfg_t, pidConfig, 1, 1);
const pid_cfg_t pidConfig_default = {
    .pid = {
        [PID_RATE_ROLL] = {50, 100, 5},      // 1340, 0, 0  800, 0, 0
        [PID_RATE_PITCH] = {50, 100, 5},     // 155, 15, 100  240, 25, 180
        [PID_RATE_YAW] = {120, 200, 0},      // 1340, 0, 0
        [PID_ANGLE_ROLL] = {100, 0, 0},     // 155, 15, 100  400, 0, 0
        [PID_ANGLE_PITCH] = {100, 0, 0},    // 1340, 0, 0   400, 0, 0
        [PID_ANGLE_YAW] = {100, 0, 0},      // 1100, 15, 100  5400, 400, 0
        [PID_POSITION_VX] = {80, 10, 50}, // 150, 0, 0
        [PID_POSITION_VY] = {80, 10, 50}, // 150, 0, 0
        [PID_POSITION_VZ] = {50, 50, 100},  // 950  0  0
        [PID_POSITION_X] = {15, 0, 0},      // 100  0  0
        [PID_POSITION_Y] = {15, 0, 0},      // 100
        [PID_POSITION_Z] = {20, 10, 0},      // 650, 0, 0
    }};
pid_obj_t pidData[PID_ITEM_COUNT];
// 重置PID参数为默认值
void pid_param_reset_default(void)
{
    // 将默认的PID参数复制到当前的PID参数中
    rt_memcpy(&pidConfig()->pid, &pidConfig_default.pid, sizeof(pidConfig_default.pid));
    // 将保存标志置为0，表示不需要保存
    pidConfig()->saveFlag = 0;
    // 保存所有配置
    zino_config_save_all();
}
void pid_Reinit_all(void)
{
    if (pidConfig()->saveFlag != PID_CONFIG_SAVE_FLAG)
    {
        rt_memcpy(&pidConfig()->pid, &pidConfig_default.pid, sizeof(pidConfig_default.pid));
        pidConfig()->saveFlag = 0;
        pidConfig()->baseThrottle = THRUST_BASE;
        pidConfig()->maxTiltAngle = MAX_TILT_ANGLE;
        pidConfig()->rcRollPitchMax = MAX_RC_ANGLE;
        pidConfig()->maxRasing = MAX_RC_RASING;
        LOG_W("loading pid config from default\n");
    }
    else
    {
        LOG_I("loading pid config from flash\n");
        for (int i = 0; i < PID_ITEM_COUNT; i++)
        {
            LOG_I("PID[%2d]={%4d,%4d,%4d}\n", i, pidConfig()->pid[i].P, pidConfig()->pid[i].I, pidConfig()->pid[i].D);
        }
        LOG_I("baseThrottle=%d\n", pidConfig()->baseThrottle);
        LOG_I("maxTiltAngle=%d\n", pidConfig()->maxTiltAngle);
        LOG_I("rcRollPitchMax=%d\n", pidConfig()->rcRollPitchMax);
        LOG_I("maxRasing=%d\n", pidConfig()->maxRasing);
    }
    for (int i = 0; i < PID_ITEM_COUNT; i++)
    {
        pidInit(&pidData[i], &pidConfig()->pid[i], 0.001f);
    }
}
void pid_param_reload(int pidIndex)
{
    if (pidIndex < PID_ITEM_COUNT)
    {
        // pidInit(&pidData[pidIndex], &pidConfig.pid[pidIndex], 0.01f);

        pidData[pidIndex].kp = pidConfig()->pid[pidIndex].P * PTERM_SCALE;
        pidData[pidIndex].ki = pidConfig()->pid[pidIndex].I * ITERM_SCALE;
        pidData[pidIndex].kd = pidConfig()->pid[pidIndex].D * DTERM_SCALE;
        // pidData[pidIndex].error = 0;
        // pidData[pidIndex].error_pre = 0;
        // pidData[pidIndex].inter = 0;
        // rt_kprintf("PID reload\n");
    }
}

void pidInit(pid_obj_t *pid, pid_param_t *param, float dt)
{
    pid->kp = param->P * PTERM_SCALE;
    pid->ki = param->I * ITERM_SCALE;
    pid->kd = param->D * DTERM_SCALE;
    pid->error = 0;
    pid->error_pre = 0;
    pid->inter = 0;
    pid->deriv = 0;
    pid->iLimit = 400;
    pid->outLimit = 400;
    pid->dt = dt;
    // rt_kprintf("PID={%4d,%4d,%4d}\n", (int)(pid->kp*1000), (int)(pid->ki*1000), (int)(pid->kd*1000));
}

float pidUpdate(pid_obj_t *pid, float error)
{
    // float output;
    pid->error = error;
    pid->inter += (pid->ki * pid->error * pid->dt);
    float d = (pid->error - pid->error_pre) / pid->dt;
    // pid->deriv = (pid->error - pid->error_pre) / pid->dt;
    pid->deriv += (d - pid->deriv) * 0.25f;
    pid->inter = constrainf(pid->inter, -pid->iLimit, pid->iLimit);
    pid->outP = pid->kp * pid->error;
    pid->outD = pid->kd * pid->deriv;
    pid->outI = pid->inter;

    // pid->outI = constrainf(pid->outI, -pid->outLimit, pid->outLimit);
    // output = pid->outP + pid->outI + pid->outD;
    // output = constrainf(output, -pid->outLimit, pid->outLimit);
    pid->out = pid->outP + pid->outI + pid->outD;
    pid->out = constrainf(pid->out, -pid->outLimit, pid->outLimit);
    pid->error_pre = pid->error;
    // pid->out = output;
    return pid->out;
}
float pid_calculate(pid_obj_t *pid, float sp, float val, float dt)
{
    if(dt < pid->min_dt) dt = pid->min_dt;

    pid->dt = dt;
   // float output;
    pid->error = sp - val;
    pid->inter += (pid->ki * pid->error * pid->dt);
    float d = (pid->error - pid->error_pre) / pid->dt;
    pid->deriv += (d - pid->deriv) * 0.25f;
    pid->inter = constrainf(pid->inter, -pid->iLimit, pid->iLimit);
    pid->outP = pid->kp * pid->error;
    pid->outD = pid->kd * pid->deriv;
    pid->outI = pid->inter;
    pid->out = pid->outP + pid->outI + pid->outD;
    pid->out = constrainf(pid->out, -pid->outLimit, pid->outLimit);
    pid->error_pre = pid->error;
    return pid->out;
}
float pid_calculate_kp(pid_obj_t *pid, float sp, float val, float dt, float kp)
{
    if(dt < pid->min_dt) dt = pid->min_dt;

    pid->dt = dt;
   // float output;
    pid->error = sp - val;
    pid->inter += (pid->ki * pid->error * pid->dt);
    float d = (pid->error - pid->error_pre) / pid->dt;
    pid->deriv += (d - pid->deriv) * 0.25f;
    pid->inter = constrainf(pid->inter, -pid->iLimit, pid->iLimit);
    pid->outP = pid->kp * pid->error * kp;
    pid->outD = pid->kd * pid->deriv;
    pid->outI = pid->inter;
    pid->out = pid->outP + pid->outI + pid->outD;
    pid->out = constrainf(pid->out, -pid->outLimit, pid->outLimit);
    pid->error_pre = pid->error;
    return pid->out;
}
float pidUpdate_yaw(pid_obj_t *pid, float error, float lpfK)
{
    // float output;
    // static float poutLast=0.f;
    pid->error = error;
    pid->inter += (pid->ki * pid->error * pid->dt);
    float d = (pid->error - pid->error_pre) / pid->dt;
    pid->deriv += (d - pid->deriv) * 0.15f;
    pid->inter = constrainf(pid->inter, -pid->iLimit, pid->iLimit);
    pid->outP += (pid->kp * pid->error - pid->outP) * lpfK;
    pid->outI = pid->inter;
    // pid->outI = constrainf(pid->outI, -pid->outLimit, pid->outLimit);
    pid->outD = pid->kd * pid->deriv;

    // output = pid->outP + pid->outI + pid->outD;
    // output = constrainf(output, -pid->outLimit, pid->outLimit);
    pid->out = pid->outP + pid->outI + pid->outD;
    pid->out = constrainf(pid->out, -pid->outLimit, pid->outLimit);
    pid->error_pre = pid->error;
    // pid->out = output;
    return pid->out;
}

void pidReset(pid_obj_t *pid)
{
    pid->error = 0;
    pid->error_pre = 0;
    pid->inter = 0;
    pid->deriv = 0;
    pid->out = 0;
    pid->outP = 0;
    pid->outI = 0;
    pid->outD = 0;
}
void pidResetAll(void)
{
    for (int i = 0; i < PID_ITEM_COUNT; i++)
    {
        pidReset(&pidData[i]);
    }
}
